import lejos.nxt.Button;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.addon.RCXLightSensor;
import lejos.util.Delay;

public class AltRobot
{
	static int NUM_SAMPLES = 10;
	static int L_SENSOR_THRESHOLD = 20;
	static int R_SENSOR_THRESHOLD = 20;
		
	boolean finished;
	int distance;
	int totalDistance;
		
	NXTRegulatedMotor motorL = new NXTRegulatedMotor(MotorPort.A);
	NXTRegulatedMotor motorR = new NXTRegulatedMotor(MotorPort.B);
	RCXLightSensor lightL = new RCXLightSensor(SensorPort.S1);
	RCXLightSensor lightR = new RCXLightSensor(SensorPort.S4);
	TouchSensor touchL = new TouchSensor(SensorPort.S2);
	TouchSensor touchR = new TouchSensor(SensorPort.S3);
		
	public AltRobot()
	{
		finished = false;
		distance = 0;
		totalDistance = 0;
		lightL.setFloodlight(true);
		lightR.setFloodlight(true);
		motorL.setSpeed(1440);
		motorR.setSpeed(1440);

		while (!Button.ESCAPE.isPressed() && !finished)
		{
			Integer valueL= lightL.getLightValue();
			Integer valueR = lightR.getLightValue();

			/*
			 * The if statements below will detect if the light sensors have
			 * detected the black circle and center itself in the middle of the
			 * circle and end the program once goal is reached.
			 */
			if (valueL <= L_SENSOR_THRESHOLD && valueR <= R_SENSOR_THRESHOLD) // both on black
			{
				motorL.rotate(480, true);
				motorR.rotate(480);
				finished = true;
			}
			else if (valueL <= L_SENSOR_THRESHOLD && valueR > R_SENSOR_THRESHOLD) // s1 is on black s2 is not
			{
				// turn right
				motorL.rotate(-5, true);
				motorR.rotate(15, true);
			}
			else if (valueL > L_SENSOR_THRESHOLD && valueR <= R_SENSOR_THRESHOLD) // s2 is on black s1 is not
			{
				// turn left
				motorR.rotate(-5, true);
				motorL.rotate(15, true);
			}

			/*
			 * What this should do is calculate the distance based upon the
			 * rotation of the wheels. We've determine that the length of the
			 * wheel is 9 1/2 inches and 1080 degrees is equal to 9 1/2 inches.
			 * Which means that 360 degrees is equal to 3 inches. When we have
			 * reached a certain distance, it will check if the sensors have
			 * detected the color black and if so, do nothing.
			 */

			else
			{					
				if (touchL.isPressed())
				{
					// robot hit a wall on left side
						
					// back up
					motorL.rotate(-540, true);
					motorR.rotate(-540);
						
					// turn to the right
					motorL.rotate(720, true);
					motorR.rotate(-720);
				}
				else if (touchR.isPressed())
				{
					// robot hit a wall on right side
						
					// back up
					motorR.rotate(-540, true);
					motorL.rotate(-540);
					
					// turn to the left
					motorR.rotate(720, true);
					motorL.rotate(-720);
				}
					
//				else
//				{
//					if(distance > 3600)
//					{
//						//turn
//						motorL.rotate(-600, true);
//						motorR.rotate(600);
//						totalDistance += distance;
//						distance = 0;
//					}
					else
					{
						motorL.rotate(30, true);
						motorR.rotate(30, true);
						distance += motorL.getPosition() - totalDistance;
					}
//				}
			}
		}
	}

	public static void main(String[] args)
	{
		AltRobot rover = new AltRobot();
	}
}
